#include "servo.h"

#define PWM_PERIOD_COUNT (1000)
void servo_init(void)
{
    DL_Timer_startCounter(SERVO_PWM_INST);
}
void servo_set_angle(uint8_t angle)
{
    // T = 20ms = 20000us ~PWM_PERIOD_COUNT
    int duty = (angle * 2000 / 180) + 500;
    int period_cnt = PWM_PERIOD_COUNT - duty * PWM_PERIOD_COUNT / 20000;
    DL_TimerG_setCaptureCompareValue(SERVO_PWM_INST, period_cnt, DL_TIMER_CC_1_INDEX);
}
void bus_servo_set_angle(uint8_t servo_idx,uint8_t angle){
    char str[10];
    sprintf(str,"%d,%d\r\n",servo_idx,angle);
    uart2_send_str(str);
}